Iterated Extended Kalman Filter Research Articles

A Monte Carlo-Based Iterative Extended Kalman Filter for Bearings-Only Tracking of Sea Targets.

In this paper, a Monte Carlo (MC)-based extended Kalman filter is proposed for a two-dimensional bearings-only tracking problem (BOT). This problem addresses the processing of noise-corrupted bearing measurements from a sea acoustic source and estimates state vectors including position and velocity. Due to the nonlinearity and complex observability properties in the BOT problem, a wide area of research has been focused on improving its state estimation accuracy. The objective of this research is to present an accurate approach to estimate the relative position and velocity of the source with respect to the maneuvering observer. This approach is implemented using the iterated extended Kalman filter (IEKF) in an MC-based iterative structure (MC-IEKF). Re-linearizing dynamic and measurement equations using the IEKF along with the MC campaign applied to the initial conditions result in significantly improved accuracy in the estimation process. Furthermore, an observability analysis is conducted to show the effectiveness of the designed maneuver of the observer. A comparison with the widely used UKF algorithm is carried out to demonstrate the performance of the proposed method.

Accurate semi‐direct lidar‐inertial odometry based on distance and normal direction

AbstractTo improve the autonomous navigation capability of unmanned platforms, an accurate semi‐direct lidar‐inertial odometry algorithm with a new point cloud alignment evaluation metric is proposed. The lidar scan points are corrected through IMU (Inertial Measurement Unit) backward propagation, and the navigation states are predicted through IMU forward propagation. In the update of the navigation states, the point cloud alignment is evaluated based on the distances and the minimum normal direction deviations between the lidar scan points and the local fitted planes of the built map, achieving accurate observations for the navigation states. The proposed algorithm updates navigation states based on the iterative extended Kalman filter. Experiments indicate that the proposed algorithm has higher positioning accuracy than the state‐of‐the‐art method, which gets smaller positioning error (7.868 m) and 52.3% improvement on average.

An Improved Adaptive Iterative Extended Kalman Filter Based on Variational Bayesian

The presence of unknown heavy-tailed noise can lead to inaccuracies in measurements and processes, resulting in instability in nonlinear systems. Various estimation methods for heavy-tailed noise exist. However, these methods often trade estimation accuracy for algorithm complexity and parameter sensitivity. To tackle this challenge, we introduced an improved variational Bayesian (VB)-based adaptive iterative extended Kalman filter. In this VB framework, the inverse Wishart distributionis used as the prior for the state prediction covariance matrix. The system state and noise parameter posterior distributions are then iteratively updated for adaptive estimation. Furthermore, we make adaptive adjustments to the IEKF filter parameters to enhance sensitivity and filtering accuracy, thus ensuring robust prediction estimation. A two-dimensional target tracking and nonlinear numerical UNGM simulation validated our algorithm. Compared to existing algorithms RKF-ML and GA-VB, our method showed significant improvements in RMSEpos and RMSEvel, with increases of 21.81% and 22.11% respectively, and a 49.04% faster convergence speed. These results highlight the method’s reliability and adaptability.

RESEARCH ON SENSOR FUSION-BASED CALIBRATION AND REAL-TIME POINT CLOUD MAPPING METHODS FOR LASER LIDAR AND IMU

Abstract. Simultaneous Localization and Mapping (SLAM), as one of the core technologies in autonomous driving, provides environment perception, decision-making, and path planning to ensure safe vehicle operation. This paper proposes a tightly-coupled fusion mapping method based on LiDAR and Inertial Measurement Unit (IMU). By introducing IMU, performing extrinsic calibration, and time synchronization with the LiDAR, the issue of mapping drift caused by inconsistent coordinate systems is addressed, leading to improved mapping accuracy. Furthermore, to compensate for IMU zero-bias errors, IMU preintegration is employed to calibrate the point cloud and optimize the initial values of the LiDAR odometry. A novel iVox voxel-based point cloud registration method is used to enhance registration efficiency without compromising mapping accuracy. Lastly, the Iterative Extended Kalman Filter (IEKF) is incorporated into the back-end state estimation to propagate and correct the estimated state using IMU data, enabling precise state estimation and map updates. This research is of significant importance in addressing the challenges of low mapping accuracy and real-time performance in single-sensor SLAM, providing an effective solution for environment perception and path planning in autonomous driving systems.

4D RADAR/IMU/GNSS INTEGRATED POSITIONING AND MAPPING FOR LARGE-SCALE ENVIRONMENTS

Abstract. 4D millimeter-wave radar can work in harsh weather conditions such as fog and snow, and measure the position and radial Doppler velocity of objects in three-dimensional space. Some existing methods perform positioning by fusing 3D velocity or point cloud matching information estimated by 4D radar with IMU information. However, the sparsity of radar point clouds and the interference of large amount of noise lead to low accuracy of odometry, and most of the existing work cannot achieve the global consistent mapping with radar point clouds. In this paper, a 4D radar/IMU/GNSS localization and mapping method, G-iRIOM, is proposed. We tightly coupled IMU measurement, Doppler observation information and point cloud matching information from 4D radar by iterative extended Kalman filtering (iEKF) method, and introduced GNSS RTK and loop closure information as global observation to correct the positioning drift of the odometry. The experimental results show that the tightly coupled Doppler velocity information can effectively improve the control of the pose on the local point cloud position, thus enhancing the mapping accuracy. Meanwhile, the introduction of GNSS and loop closure information can significantly improve the positioning accuracy of 4D radar odometry as a kind of global observation, and achieve global consistent mapping of large-scale outdoor scenes based on 4D radar point clouds.

Rotating 3D laser mapping system for multi-rotor drones

Accurate positional estimation is an essential prerequisite for the regular operation of an autonomous rotary-wing Unmanned Aerial Vehicles (UAV). However, the field of view (FOV) limitation problem of lidar makes it more challenging to locate the rotary-wing UAV in an unknown environment. To address rotor drones with an insufficient FOV and the observation blindness of lidar in complex environments, this paper designs a rotorcraft UAV system based on rotating 3D lidar and proposes a simultaneous localization and mapping algorithm for rotating 3D lidar. The algorithm distinguishes between planar and edge features based on the curvature value of the point cloud first. Then, to reduce the impact caused by the UAV motion and lidar rotation, messages about the Inertial Measurement Unit (IMU) and real-time rotation angles are used to compensate for these motions twice, while the IMU measurements are used for state prediction, and the error-sate iterative extended Kalman filter is used to update the residuals after matching line and surface features with sub-map. Finally, Smoother high-rate odometer data was obtained through IMU pre-integration and a first-order low-pass filter. The experimental results show that the proposed rotating lidar unit in indoor and outdoor conditions makes the rotorcraft UAV have a larger FOV, which not only improves the environmental perception capability and positional estimation accuracy of the rotorcraft but enhances the positioning reliability and flight stability of the rotorcraft UAV in complex environments.

Multi-source-data state estimation for integrated electricity–heat-building networks

Building heating systems (BHS) play a major role in integrated electricity and heat systems (IEHS). To capture the real-time dynamic operating state of IEHS based on multi-source measurement data, this paper develops a comprehensive state estimation (SE) framework for IEHS with BHS fleets. First, a thermo-electrical SE model integrating BHS, electric power grid (EPG), and district heating network (DHN) is developed. Then, to jointly estimate the state variables of IEHS with different time scales of dynamics, a holistic SE algorithm is proposed based on the partial equivalence between the Weighted Least Squares (WLS) estimation problem and the correction stage of the Iterative Extended-Kalman Filter (IEKF). It also includes a data anomaly processing algorithm to combat data corruptions from sensing and communication processes. Simulation results show that the proposed framework can accurately track the thermo-electrical states of the system both in the presence of measurement noise and anomalies during various dynamics in IEHS.

4D iRIOM: 4D Imaging Radar Inertial Odometry and Mapping

Millimeter wave radar can measure distances, directions, and Doppler velocity for objects in harsh conditions such as fog. The 4D imaging radar with both vertical and horizontal data resembling an image can also measure objects' height. Previous studies have used 3D radars for ego-motion estimation. But few methods leveraged the rich data of imaging radars, and they usually omitted the mapping aspect, thus leading to inferior odometry accuracy. This paper presents a real-time imaging radar inertial odometry and mapping method, iRIOM, based on the submap concept. To deal with moving objects and multipath reflections, we use the graduated non-convexity method to robustly and efficiently estimate ego-velocity from a single scan. To measure the agreement between sparse non-repetitive radar scan points and submap points, the distribution-to-multi-distribution distance for matches is adopted. The ego-velocity, scan-to-submap matches are fused with the 6D inertial data by an iterative extended Kalman filter to get the platform's 3D position and orientation. A loop closure module is also developed to curb the odometry module's drift. To our knowledge, iRIOM based on the two modules is the first 4D radar inertial SLAM system. On our and third-party data, we show iRIOM's favorable odometry accuracy and mapping consistency against the FastLIO-SLAM and the EKFRIO. Also, the ablation study reveal the benefit of inertial data versus the constant velocity model, and scan-to-submap matching versus scan-to-scan matching.

A low‐drift and real‐time localisation and mapping method for handheld hemispherical view LiDAR‐IMU integration system

AbstractThis paper proposes a simultaneous localisation and mapping (SLAM) framework that uses a handheld hemispherical view LiDAR‐IMU integration system. Inspired by the specific characteristic of the hemispherical view LiDAR, a ground segmentation module based on seed points is designed. The ground points are then downsampled to eliminate redundant vertical constraints. The IMU data and the pre‐processed point cloud are used to perform state estimation via a tightly coupled iterative extended Kalman filter (iEKF) to obtain the pose estimation. The automatically detected loop closures provide closed‐loop constraints for the odometry, and a factor graph ensures the global consistency of the map. Data from diverse scenes are collected via a prototype system. Both qualitative and quantitative experiments are carried out to verify the framework's performance. According to the experimental results, our framework achieves low‐drift, high‐coverage and real‐time performance, outperforming the state‐of‐the‐art LiDAR SLAM methods in our handheld hemispherical view LiDAR‐IMU test sites. For the research community's benefit, the dataset is publicly provided for other researchers to compare against.

Improving the Computational Efficiency of ROVIO

ROVIO is one of the state-of-the-art monocular visual inertial odometry algorithms. It uses an Iterative Extended Kalman Filter (IEKF) to align visual features and update the vehicle state simultaneously by including the feature locations in the state vector of the IEKF. This algorithm is single-core intensive, which allows the other cores to be used for other algorithms, such as object detection and path optimization. However, the computational cost of the algorithm grows rapidly with the maximum number of features to track. Each feature adds three new states (a 2D bearing vector and inverse depth), leading to bigger matrix multiplications that are computationally expensive. The main computational load of ROVIO is the iterative update step of the IEKF. In this work, we reduce the average computational cost of ROVIO by  [Formula: see text] on an NVIDIA Jetson TX2, without affecting the accuracy of the algorithm. This computational gain is mainly achieved by exploiting the sparse matrices in ROVIO. Furthermore, we reduce the computational peaks by pre-selecting new features based on their already calculated FAST score. The combination of both modifications allows us to run ROVIO on the computationally restricted Raspberry Pi Zero 2[Formula: see text]W.

State-of-charge estimation of lithium-ion batteries based on fractional-order modeling and adaptive square-root cubature Kalman filter

This paper mainly studies the state of charge (SOC) estimation of lithium batteries based on a fractional-order adaptive square-root cubature Kalman filter (FO-ASRCKF). Firstly, a fractional-order model (FOM) of lithium battery is established by using fractional-order derivative theory. In order to meet the identification accuracy, an improved adaptive genetic algorithm is applied to the process of multi-parameter model identification. Then, the FO-ASRCKF algorithm based on FOM and adaptive rules is proposed, and a comparative experiment with Fractional-order adaptive iterative extended Kalman filter (FO-AIEKF) and Integer-order adaptive square-root cubature Kalman filter (IO-ASRCKF) is carried out. The experimental results show that the proposed FO-ASRCKF can work normally under various working conditions, and it has higher SOC estimation accuracy, with the mean absolute error (MAE) being less than 0.5%. Moreover, it can also overcome the divergence caused by noise and wrong initial values, indicating a better robustness.

Dynamic Harmonic Estimation Using a Novel Robust Filtering Strategy: Iterated Sliding Innovation Cubature Filter

Active filters (AFs) are effective tools for mitigating the detrimental effects of harmonic components on the power systems. The performance of AFs is significantly dependent on designing an accurate and robust estimator which is responsible for providing reference harmonic values. In this article, a novel technique, called sliding innovation (SI) cubature filter, is proposed to estimate the harmonic parameters, i.e., magnitude and phase, in various operating conditions. The proposed method exploits the concept of sliding mode control in the formulation of the measurement update step in the Bayesian filtering framework to enhance the robustness of the estimator. Furthermore, the iterated version of the proposed algorithm, called iterative SI cubature filter, is presented to enhance the accuracy of the estimator. The proposed method keeps its robustness and accuracy in the noisy conditions under the fault occurrence as well as power system transients. The obtained results from both simulation and experimental setup confirm that the proposed estimator is more accurate and robust with a higher convergence speed compared to the well-known discrete Fourier transform (DFT), cubature Kalman filter (CKF), iterated extended Kalman filter (IEKF), and particle filter (PF).

An Expectation-Maximization-Based Estimation Algorithm for AOA Target Tracking With Non-Gaussian Measurement Noises

This paper considers angle-of-arrival (AOA) target tracking in the two-dimensional plane with non-Gaussian noise. Practical situations arise where sensor measurement noise is non-Gaussian and the standard extended Kalman filter (EKF) based tracker may not be robust, resulting in large tracking error or even lack of convergence. This motivated development of a new tracker, designed for white non-Gaussian noise cases. The noise is decomposed into a sum of Gaussian components using expectation maximization (EM), and the target state is estimated by adapting an extended Kalman filter (EKF). Adaptations are necessary to combine the EM with the iterative EKF. From the analysis of the estimation error, a closed-form expression was derived revealing the presence of a bias in the estimate. On investigation, a sensor path optimization scheme is found that can eliminate bias. Furthermore, a compensation algorithm is presented, which reduces bias for the case of static sensors. The new tracker is suitable for real-time implementation and simulation results demonstrate significant improvements for the target tracking with non-Gaussian noise.

Outlier-Robust Iterative Extended Kalman Filtering

In this paper, we develop OR-IEKF which is a novel outlier-robust iterative extended Kalman filtering (IEKF) framework based on nonlinear regression formulation of update step. A new Kalman-type update step with reweighted prediction covariance and reweighted observation noise covariance are produced under the OR-IEKF framework, which could cut off the large outliers in observations causing by unknown outlier noises. By using various robust cost functions to solve such special nonlinear regression problems, we derive three algorithms. The performances of these new filters are evaluated in a nonlinear system simulation study.

OMC-SLIO: Online Multiple Calibrations Spinning LiDAR Inertial Odometry

Light detection and ranging (LiDAR) is often combined with an inertial measurement unit (IMU) to get the LiDAR inertial odometry (LIO) for robot localization and mapping. In order to apply LIO efficiently and non-specialistically, self-calibration LIO is a hot research topic in the related community. Spinning LiDAR (SLiDAR), which uses an additional rotating mechanism to spin a common LiDAR and scan the surrounding environment, achieves a large field of view (FoV) with low cost. Unlike common LiDAR, in addition to the calibration between the IMU and the LiDAR, the self-calibration odometer for SLiDAR must also consider the mechanism calibration between the rotating mechanism and the LiDAR. However, existing self-calibration LIO methods require the LiDAR to be rigidly attached to the IMU and do not take the mechanism calibration into account, which cannot be applied to the SLiDAR. In this paper, we propose firstly a novel self-calibration odometry scheme for SLiDAR, named the online multiple calibration inertial odometer (OMC-SLIO) method, which allows online estimation of multiple extrinsic parameters among the LiDAR, rotating mechanism and IMU, as well as the odometer state. Specially, considering that the rotating and static parts of the motor encoder inside the SLiDAR are rigidly connected to the LiDAR and IMU respectively, we formulate the calibration within the SLiDAR as two separate sets of calibrations: the mechanism calibration between the LiDAR and the rotating part of the motor encoder and the sensor calibration between the static part of the motor encoder and the IMU. Based on such a SLiDAR calibration formulation, we can construct a well-defined kinematic model from the LiDAR to the IMU with the angular information from the motor encoder. Based on the kinematic model, a two-stage motion compensation method is presented to eliminate the point cloud distortion resulting from LiDAR spinning and platform motion. Furthermore, the mechanism and sensor calibration as well as the odometer state are wrapped in a measurement model and estimated via an error-state iterative extended Kalman filter (ESIEKF). Experimental results show that our OMC-SLIO is effective and attains excellent performance.

Quaternion-Based Iterative Extended Kalman Filter for Sensor Fusion of Vision Sensor and IMU in 6-DOF Displacement Monitoring

As civil structures are exposed to various external loads, their periodic evaluation is paramount to ensure their safety. By estimating the 6-degree-of-freedom (DOF) displacement of structures, structural behavior can be monitored directly. Therefore, this study aims to develop a translational and rotational displacement estimation method by fusing a vision sensor and inertial measurement unit (IMU) using a quaternion-based iterative extended Kalman filter (QIEKF). The QIEKF algorithm was applied to reduce the nonlinear influence on the measurement model. The 6-DOF displacement is predicted using the integral of the gyroscope output and updated via a combination of an accelerometer and a magnetometer through a vector matching process in the Kalman filter framework. Subsequently, the 6-DOF displacement estimation result is updated through a vision sensor using a 2-D planar marker and homography transformation in the Kalman filter framework. The performance of the proposed sensor fusion method was verified with experiments using a motorized motion stage, and the results show that the displacements can be estimated with high accuracy regardless of measurement noise and slowly varying signal drift.

State of Charge Estimation of Lithium-Ion Batteries Based on an Adaptive Iterative Extended Kalman Filter for AUVs

As a power source for autonomous underwater vehicles (AUVs), lithium-ion batteries play an important role in ensuring AUVs' electric power propulsion performance. An accurate state of charge (SOC) estimation method is the key to achieving energy optimization for lithium-ion batteries. Due to the complicated ocean environments, traditional filtering methods cannot effectively estimate the SOC of lithium-ion batteries in an AUV. Based on the standard extended Kalman filter (EKF), an adaptive iterative extended Kalman filter (AIEKF) method for the SOC in an AUV is proposed to address the traditional filter's problems, such as low accuracy and large errors. In this method, the adaptive update is introduced to deal with the uncertain noise from the lithium-ion battery. The iteration is used to improve the convergence speed and to reduce the computational burden. Compared with the EKF, iterative extended Kalman filter (IEKF) and adaptive extended Kalman filter (AEKF), the proposed AIEKF has a higher estimation accuracy and anti-interference capability, which is suitable for the AUV's SOC estimation. In addition, based on the second-order equivalent circuit model of the lithium-ion battery, a forgetting factor recursive least squares (FFRLS) method is proposed to deal with the multi-variability problem. In the end, four different methods, including EKF, IEKF, AEKF, and the proposed AIEKF, are compared in computational time. The experiment results show that the proposed method has high accuracy and fast estimation speed, meaning that it has good application potential in AUVs.

Thermo-Electrical state and demand response potential estimation for power systems with building thermostats

Heating and cooling are major resources of Demand Response (DR) to enhance the flexibility and reliability of power grids. In order to maximize their potential, it is necessary to reliably keep track of their operating states in real-time operation. This paper presents a comprehensive state estimation framework for power systems with building thermostats, with a tractable thermodynamic building model and integration of multi-source information from weather, power grid, building systems. Based on the thermodynamic model and state of the buildings, the building temperature trajectories in the next few hours can be accurately predicted, such that the DR potential of the building can be precisely estimated. To jointly estimate the state variables of power grids and state variables in building thermostats with different time scales of dynamics, a holistic estimation framework is developed based on the partial equivalence between the Weighted Least Squares (WLS) estimation problem and the correction stage of the Iterative Extended-Kalman Filter (IEKF). Simulation results show that the proposed framework can accurately track the thermo-electrical states of the system and estimate the DR potential in the presence of measurement noise and bad data.

UWB/INS-based robust anchor-free relative positioning scheme for UGVs

With the development of new distributed cluster applications such as distributed radar and UGV cooperation, the concept of relative positioning springs up—different from the classic positioning application, relative positioning focuses on the real-time determination of relative position relationships in multi-node networks without pre-deployment anchor, prior map and GNSS. Under such application conditions, relative localization also requires high real-time performance, and high accuracy in short baseline scenes, and can solve the positioning failure caused by occlusion and other factors. A high-precision relative positioning scheme based on UWB-INS fusion is proposed in this paper: a robust relative positioning algorithm based on iterative extended Kalman filter is optimized by modeling the system error state, and the adaptive residual elimination algorithm is introduced to ensure the robustness of positioning service. A series of experiments show that the scheme proposed in this paper can provide centimeter-level real-time positioning service for multi-node dynamic networks without anchors and maps, and is expected to become a potential effective relative positioning technology scheme.

Outlier-resilient iterative-extended Kalman filter based on maximum correntropy criterion

The extended Kalman filter (EKF) is the most popular tool for state estimation of nonlinear stochastic systems. The conventional EKF can be refined via iterative re-linearization, leading to the so-called iterative EKF (IEKF). The performance of either EKF or IEKF will deteriorate in the presence of outliers. In this article, an outlier-resilient IEKF method is proposed using a more robust cost function, the correntropy, to replace the traditional mean squared error criterion. An iterative procedure is derived to maximize the correntropy criterion in a similar way to the Gauss–Newton optimization. Simulation results demonstrate the superiority of the proposed method as compared to the existing methods.